{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "df27d777",
   "metadata": {},
   "source": [
    "# Robotics, Vision & Control 3e: for Python\n",
    "## Chapter 7: Robot Arm Kinematics\n",
    "\n",
    "Copyright (c) 2021- Peter Corke"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9836bfc3",
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "    from google.colab import output\n",
    "    print('Running on CoLab')\n",
    "    output.enable_custom_widget_manager()\n",
    "    !pip install ipympl\n",
    "    !pip install roboticstoolbox-python>=1.0.2\n",
    "    !pip install --no-deps rvc3python\n",
    "    COLAB = True\n",
    "except ModuleNotFoundError:\n",
    "    COLAB = False\n",
    "\n",
    "from IPython.core.interactiveshell import InteractiveShell\n",
    "InteractiveShell.ast_node_interactivity = \"last_expr_or_assign\"\n",
    "from IPython.display import HTML\n",
    "\n",
    "%matplotlib widget\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "# add RTB examples folder to the path\n",
    "import sys, os.path\n",
    "import RVC3 as rvc\n",
    "sys.path.append(os.path.join(rvc.__path__[0], 'models'))\n",
    "\n",
    "# helper function to run bdsim in a subprocess and transfer results using a pickle file\n",
    "import pickle\n",
    "def run_shell(tool, **params):\n",
    "    global out\n",
    "    pyfile = os.path.join(rvc.__path__[0], \"models\", tool+\".py\")\n",
    "    cmd = f\"python {pyfile} -H +a -o\"\n",
    "    for key, value in params.items():\n",
    "        cmd += f' --global \"{key}={value}\"'\n",
    "    print(cmd)\n",
    "    os.system(cmd)\n",
    "    with open(\"bd.out\", \"rb\") as f:\n",
    "        out = pickle.load(f)\n",
    "        \n",
    "# ------ standard imports ------ #\n",
    "import numpy as np\n",
    "from scipy import optimize\n",
    "import math\n",
    "from math import pi\n",
    "np.set_printoptions(\n",
    "    linewidth=120, formatter={\n",
    "        'float': lambda x: f\"{0:8.4g}\" if abs(x) < 1e-10 else f\"{x:8.4g}\"})\n",
    "np.random.seed(0)\n",
    "from spatialmath import *\n",
    "from spatialmath.base import *\n",
    "from roboticstoolbox import *"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "73955b81",
   "metadata": {},
   "source": [
    "# 7.1 Forward Kinematics\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3f91f95d",
   "metadata": {},
   "source": [
    "## 7.1.1 Forward kinematics from a pose graph\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ceb43952",
   "metadata": {},
   "source": [
    "### 7.1.1.1 2-Dimensional (Planar) Robotic Arms\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9c6da426",
   "metadata": {},
   "outputs": [],
   "source": [
    "a1 = 1;\n",
    "e = ET2.R() * ET2.tx(a1);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a863cc1a",
   "metadata": {},
   "outputs": [],
   "source": [
    "len(e)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "833d969c",
   "metadata": {},
   "outputs": [],
   "source": [
    "e"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e5da4d1d",
   "metadata": {},
   "outputs": [],
   "source": [
    "e.fkine(pi / 6)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6564bc09",
   "metadata": {},
   "outputs": [],
   "source": [
    "SE2.Rot(pi / 6) * SE2.Tx(a1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "caab97dd",
   "metadata": {},
   "outputs": [],
   "source": [
    "# e.teach(0)  # not working in Jupyter\n",
    "e.plot(0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "472b977f",
   "metadata": {},
   "outputs": [],
   "source": [
    "a1 = 1; a2 = 1;\n",
    "e = ET2.R() * ET2.tx(a1) * ET2.R() * ET2.tx(a2);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "69abd7f6",
   "metadata": {},
   "outputs": [],
   "source": [
    "e"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b8291d6b",
   "metadata": {},
   "outputs": [],
   "source": [
    "e.fkine(np.deg2rad([30, 40])).printline()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7de0a0e9",
   "metadata": {},
   "outputs": [],
   "source": [
    "T = SE2.Rot(np.deg2rad(30)) * SE2.Tx(a1) * SE2.Rot(np.deg2rad(40)) * SE2.Tx(a2);\n",
    "T.printline()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1d522933",
   "metadata": {},
   "outputs": [],
   "source": [
    "e.n\n",
    "e.joints()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "952d8f34",
   "metadata": {},
   "outputs": [],
   "source": [
    "e.structure"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3cada374",
   "metadata": {},
   "outputs": [],
   "source": [
    "e.plot(np.deg2rad([30, 40]));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7d0baf9a",
   "metadata": {},
   "outputs": [],
   "source": [
    "e[1]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "797c123b",
   "metadata": {},
   "outputs": [],
   "source": [
    "e[1].eta\n",
    "e[1].A()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "aa47d68b",
   "metadata": {},
   "outputs": [],
   "source": [
    "e = ET2.R() * ET2.tx(qlim=[1,2])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cc84e466",
   "metadata": {},
   "outputs": [],
   "source": [
    "e.structure"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ed75c7e4",
   "metadata": {},
   "source": [
    "### 7.1.1.2 3-Dimensional Robotic Arms\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "82a4b294",
   "metadata": {},
   "outputs": [],
   "source": [
    "a1 = 1; a2 = 1;\n",
    "e = ET.Rz() * ET.Ry() \\\n",
    "     * ET.tz(a1) * ET.Ry() * ET.tz(a2) \\\n",
    "     * ET.Rz() * ET.Ry() * ET.Rz();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "36aa9974",
   "metadata": {},
   "outputs": [],
   "source": [
    "e.n\n",
    "e.structure"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "29e9a28b",
   "metadata": {},
   "outputs": [],
   "source": [
    "e.fkine(np.zeros((6,)))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c9212ce5",
   "metadata": {},
   "source": [
    "## 7.1.2 Forward kinematics as a chain of robot links\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "196dbf8e",
   "metadata": {},
   "source": [
    "### 7.1.2.1 2-Dimensional (Planar) case\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "77190b24",
   "metadata": {},
   "outputs": [],
   "source": [
    "link1 = Link2(ET2.R(), name=\"link1\");\n",
    "link2 = Link2(ET2.tx(1) * ET2.R(), name=\"link2\", parent=link1);\n",
    "link3 = Link2(ET2.tx(1), name=\"link3\", parent=link2);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "85bf3bf7",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = ERobot2([link1, link2, link3], name=\"my robot\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c04b06c9",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.fkine(np.deg2rad([30, 40])).printline()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1c1d0ed2",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.plot(np.deg2rad([30, 40]));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fb0bd95b",
   "metadata": {},
   "outputs": [],
   "source": [
    "q = np.array([np.linspace(0, pi, 100), np.linspace(0, -2 * pi, 100)]).T;\n",
    "q.shape\n",
    "robot.plot(q);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "59c6e900",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot[1]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d6156447",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot[\"link2\"]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ba45b391",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.ee_links"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "24fa72f8",
   "metadata": {},
   "outputs": [],
   "source": [
    "link2.parent"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8fb199f2",
   "metadata": {},
   "outputs": [],
   "source": [
    "link2.children"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4a368422",
   "metadata": {},
   "outputs": [],
   "source": [
    "link2.jindex"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "38440457",
   "metadata": {},
   "outputs": [],
   "source": [
    "link2.isrevolute\n",
    "link2.isprismatic"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "42ba3b39",
   "metadata": {},
   "outputs": [],
   "source": [
    "print(link2.qlim)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "69753ff2",
   "metadata": {},
   "outputs": [],
   "source": [
    "link2.A(pi / 6)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4a0c7809",
   "metadata": {},
   "outputs": [],
   "source": [
    "link2.ets"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "37f52b17",
   "metadata": {},
   "source": [
    "### 7.1.2.2 3-Dimensional case\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c21cb5ae",
   "metadata": {},
   "outputs": [],
   "source": [
    "a1 = 1; a2 = 1;\n",
    "robot6 = ERobot(ET.Rz() * ET.Ry() * ET.tz(a1) * ET.Ry() \\\n",
    "                * ET.tz(a2) * ET.Rz() * ET.Ry() * ET.Rz())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d4b68cec",
   "metadata": {},
   "outputs": [],
   "source": [
    "models.list(type=\"ETS\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a95f5559",
   "metadata": {},
   "outputs": [],
   "source": [
    "panda = models.ETS.Panda()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "aa8ecac0",
   "metadata": {},
   "outputs": [],
   "source": [
    "panda.qr"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fbbb7053",
   "metadata": {},
   "outputs": [],
   "source": [
    "panda.addconfiguration(\"foo\", [1, 2, 3, 4, 5, 6, 7]) "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3911ba03",
   "metadata": {},
   "outputs": [],
   "source": [
    "panda.configs[\"foo\"];\n",
    "panda.configs[\"qz\"];"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cfe58d64",
   "metadata": {},
   "outputs": [],
   "source": [
    "panda.fkine(panda.qr).printline()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "77e74447",
   "metadata": {},
   "outputs": [],
   "source": [
    "panda.plot(panda.qr);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5319dc43",
   "metadata": {},
   "outputs": [],
   "source": [
    "T = panda.fkine_all(panda.qr);\n",
    "len(T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6669ee7b",
   "metadata": {},
   "outputs": [],
   "source": [
    "T[1].printline()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ed6a3ab3",
   "metadata": {},
   "source": [
    "### 7.1.2.3 Tools and bases\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "62c5a5e8",
   "metadata": {},
   "outputs": [],
   "source": [
    "panda.base = SE3.Tz(3) * SE3.Rx(pi); # robot is 3m up and hanging down\n",
    "panda.tool = SE3.Tz(0.15); # tool is 150mm long in z-direction"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2a9fd255",
   "metadata": {},
   "outputs": [],
   "source": [
    "panda.fkine(panda.qr, tool=SE3.Trans(0.05, 0.02, 0.20) * SE3.Rz(np.deg2rad(45)));"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b21c259c",
   "metadata": {},
   "source": [
    "## 7.1.3 Branched robots\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "26fe3d25",
   "metadata": {},
   "source": [
    "### 7.1.3.1 2D (Planar) Branched robots\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "97b2ddeb",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = ERobot2([\n",
    "  Link2(ET2.R(), name=\"link1\"),\n",
    "  Link2(ET2.tx(1) * ET2.tx(1.2) * ET2.ty(-0.5) * ET2.R(), name=\"link2\", parent=\"link1\"),\n",
    "  Link2(ET2.tx(1), name=\"ee_1\", parent=\"link2\"),\n",
    "  Link2(ET2.tx(1) * ET2.tx(0.6) * ET2.ty(0.5) * ET2.R(), name=\"link3\", parent=\"link1\"),\n",
    "  Link2(ET2.tx(1), name=\"ee_2\", parent=\"link3\") ], name=\"branched\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "23c77c40",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot[\"link1\"].children"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9fba6ccd",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "916c9931",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.showgraph()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "083c61a1",
   "metadata": {},
   "outputs": [],
   "source": [
    "# robot.teach()\n",
    "plt.figure()\n",
    "robot.plot([0.3, 0.4, -0.6]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "85ca2de4",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.fkine([0.3, 0.4, -0.6], end=\"ee_2\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f4be9a77",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.fkine([0.3, 0.4, -0.6], end=\"ee_2\", start=\"ee_1\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1a1918db",
   "metadata": {},
   "source": [
    "## 7.1.4 Unified Robot Description Format (URDF)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c8b03b76",
   "metadata": {},
   "outputs": [],
   "source": [
    "urdf, *_ = ERobot.URDF_read(\"ur_description/urdf/ur5_joint_limited_robot.urdf.xacro\")\n",
    "urdf"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "90c2bff3",
   "metadata": {},
   "outputs": [],
   "source": [
    "ur5 = models.URDF.UR5()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4f5c93f0",
   "metadata": {},
   "outputs": [],
   "source": [
    "ur5.showgraph()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9ae2922c",
   "metadata": {},
   "outputs": [],
   "source": [
    "ur5.grippers"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "327e1237",
   "metadata": {},
   "outputs": [],
   "source": [
    "ur5.plot(ur5.qr);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f82fb5b0",
   "metadata": {},
   "outputs": [],
   "source": [
    "ur5.dynamics()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b4c29c5d",
   "metadata": {},
   "outputs": [],
   "source": [
    "yumi = models.URDF.YuMi()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c8801741",
   "metadata": {},
   "outputs": [],
   "source": [
    "yumi.showgraph(ets=\"brief\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "eda4f296",
   "metadata": {},
   "outputs": [],
   "source": [
    "yumi.grippers"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "76d53924",
   "metadata": {},
   "outputs": [],
   "source": [
    "yumi.plot(yumi.q1);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fc0faaa8",
   "metadata": {
    "lines_to_next_cell": 2
   },
   "outputs": [],
   "source": [
    "models.list(type=\"URDF\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "10568365",
   "metadata": {},
   "outputs": [],
   "source": [
    "pr2 = models.URDF.PR2()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "94292f5a",
   "metadata": {},
   "outputs": [],
   "source": [
    "pr2.showgraph()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "66d2070c",
   "metadata": {},
   "outputs": [],
   "source": [
    "pr2.plot(pr2.qz)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "66a0fc01",
   "metadata": {},
   "source": [
    "## 7.1.5 Denavit-Hartenberg Parameters\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "274dd13b",
   "metadata": {},
   "outputs": [],
   "source": [
    "link = RevoluteDH(a=1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ad7b5971",
   "metadata": {},
   "outputs": [],
   "source": [
    "link.A(0.5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "673cd40c",
   "metadata": {
    "lines_to_next_cell": 2
   },
   "outputs": [],
   "source": [
    "models.list(type=\"DH\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "716a54e5",
   "metadata": {},
   "outputs": [],
   "source": [
    "irb140 = models.DH.IRB140();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c025ffb4",
   "metadata": {},
   "outputs": [],
   "source": [
    "irb140"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "05ac6213",
   "metadata": {},
   "outputs": [],
   "source": [
    "irb140._tool"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9ae3fff4",
   "metadata": {},
   "outputs": [],
   "source": [
    "irb140.fkine(irb140.qr).printline(\"rpy/xyz\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ab3e6c60",
   "metadata": {},
   "outputs": [],
   "source": [
    "irb140.plot(irb140.qr);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dfb184db",
   "metadata": {},
   "outputs": [],
   "source": [
    "# irb140.teach()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ab41bdfa",
   "metadata": {},
   "outputs": [],
   "source": [
    "irb140.ets()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "940e2659",
   "metadata": {},
   "source": [
    "# 7.2 Inverse Kinematics\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "61113591",
   "metadata": {},
   "source": [
    "## 7.2.1 2-Dimensional (Planar) Robotic Arms\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3a28aced",
   "metadata": {},
   "source": [
    "### 7.2.1.1 Closed-Form Solution\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "48b379cd",
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy\n",
    "a1, a2 = sympy.symbols(\"a1 a2\")\n",
    "e = ET2.R() * ET2.tx(a1) * ET2.R() * ET2.tx(a2);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b8dac101",
   "metadata": {},
   "outputs": [],
   "source": [
    "q0, q1 = sympy.symbols(\"q0 q1\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dafa6650",
   "metadata": {},
   "outputs": [],
   "source": [
    "TE = e.fkine([q0, q1])\n",
    "x_fk, y_fk = TE.t;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "aeed4709",
   "metadata": {},
   "outputs": [],
   "source": [
    "x, y = sympy.symbols(\"x y\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6a678fbe",
   "metadata": {},
   "outputs": [],
   "source": [
    "eq1 = (x_fk**2 + y_fk**2 - x**2 - y**2).trigsimp()  "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d1a658a7",
   "metadata": {},
   "outputs": [],
   "source": [
    "q1_sol = sympy.solve(eq1, q1) \n",
    "eq0 = tuple(map(sympy.expand_trig, [x_fk - x, y_fk - y]))\n",
    "q0_sol = sympy.solve(eq0, [sympy.sin(q0), sympy.cos(q0)]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a0c70d23",
   "metadata": {},
   "outputs": [],
   "source": [
    "sympy.atan2(q0_sol[sympy.sin(q0)], q0_sol[sympy.cos(q0)]).simplify()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7cb6af43",
   "metadata": {},
   "source": [
    "### 7.2.1.2 Numerical Solution\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0d1cdc16",
   "metadata": {},
   "outputs": [],
   "source": [
    "e = ET2.R() * ET2.tx(1) * ET2.R() * ET2.tx(1);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4e9c1556",
   "metadata": {},
   "outputs": [],
   "source": [
    "pstar = np.array([0.6, 0.7]);  # desired position\n",
    "E = lambda q: np.linalg.norm(e.fkine(q).t - pstar);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "866fd358",
   "metadata": {},
   "outputs": [],
   "source": [
    "sol = optimize.minimize(E, [0, 0]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4da1d1a0",
   "metadata": {},
   "outputs": [],
   "source": [
    "sol.x"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "44710a27",
   "metadata": {},
   "outputs": [],
   "source": [
    "e.fkine(sol.x).printline()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8b920216",
   "metadata": {},
   "source": [
    "## 7.2.2 3-Dimensional Robotic Arms\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7832e032",
   "metadata": {},
   "source": [
    "### 7.2.2.1 Closed-Form Solution\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "54672446",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma = models.DH.Puma560();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bda6dff0",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.qn"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "37a85873",
   "metadata": {},
   "outputs": [],
   "source": [
    "T = puma.fkine(puma.qn);\n",
    "T.printline()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "74eb164d",
   "metadata": {},
   "outputs": [],
   "source": [
    "sol = puma.ikine_a(T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c281ec59",
   "metadata": {},
   "outputs": [],
   "source": [
    "sol.q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "977cc543",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.fkine(sol.q).printline()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bbf904fb",
   "metadata": {},
   "outputs": [],
   "source": [
    "sol = puma.ikine_a(T, \"r\");\n",
    "sol.q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c3652d69",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.ikine_a(SE3.Tx(3))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fefcbbd6",
   "metadata": {},
   "outputs": [],
   "source": [
    "q = [0, pi/4, pi, 0.1, 0, 0.2];"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2e65e4a1",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.ikine_a(puma.fkine(q), \"ru\").q"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f3186d9a",
   "metadata": {},
   "source": [
    "### 7.2.2.2 Numerical Solution\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7b9bcd58",
   "metadata": {},
   "outputs": [],
   "source": [
    "T = puma.fkine(puma.qn);\n",
    "T.printline(\"rpy/xyz\")\n",
    "sol = puma.ikine_LM(T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "38fcf064",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.qn"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6b29bcb8",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.fkine(sol.q).printline(\"rpy/xyz\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "246f422c",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.plot(sol.q);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fab2daa8",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.ikine_LM(T, q0=[0, 0, 3, 0, 0, 0])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0a5ff963",
   "metadata": {},
   "source": [
    "## 7.2.3 Underactuated Manipulator\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "472fb6de",
   "metadata": {},
   "outputs": [],
   "source": [
    "cobra = models.DH.Cobra600()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9391604f",
   "metadata": {},
   "outputs": [],
   "source": [
    "TE = SE3.Trans(0.4, -0.3, 0.2) * SE3.RPY(np.deg2rad([30, 0, 170]), order=\"xyz\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "76a4293c",
   "metadata": {},
   "outputs": [],
   "source": [
    "sol = cobra.ikine_LM(TE)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f04da090",
   "metadata": {},
   "outputs": [],
   "source": [
    "sol = cobra.ikine_LM(TE, mask=[1, 1, 1, 0, 0, 1])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "35fccf74",
   "metadata": {},
   "outputs": [],
   "source": [
    "cobra.fkine(sol.q).printline(\"rpy/xyz\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a95598e0",
   "metadata": {},
   "outputs": [],
   "source": [
    "TE.plot(color=\"r\");\n",
    "cobra.fkine(sol.q).plot(color=\"b\");\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b1026eeb",
   "metadata": {},
   "source": [
    "## 7.2.4 Overactuated (Redundant) Manipulator\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "40e220cc",
   "metadata": {},
   "outputs": [],
   "source": [
    "panda = models.ETS.Panda();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2cc1b65b",
   "metadata": {},
   "outputs": [],
   "source": [
    "TE = SE3.Trans(0.7, 0.2, 0.1) * SE3.OA((0, 1, 0), (0, 0, -1));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "25f1cb25",
   "metadata": {},
   "outputs": [],
   "source": [
    "sol = panda.ikine_LM(TE)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b8088ed2",
   "metadata": {},
   "outputs": [],
   "source": [
    "panda.fkine(sol.q).printline(\"angvec\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "10519034",
   "metadata": {},
   "source": [
    "# 7.3 Trajectories\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c1a3b44b",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma = models.DH.Puma560();"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ba31a717",
   "metadata": {},
   "source": [
    "## 7.3.1 Joint-Space Motion\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0a220410",
   "metadata": {},
   "outputs": [],
   "source": [
    "TE1 = SE3.Trans(0.4, -0.2, 0) * SE3.Rx(3);\n",
    "TE2 = SE3.Trans(0.4, 0.2, 0) * SE3.Rx(1);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1ebd5ab0",
   "metadata": {},
   "outputs": [],
   "source": [
    "sol1 = puma.ikine_a(TE1, \"ru\");\n",
    "sol2 = puma.ikine_a(TE2, \"ru\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "129c890b",
   "metadata": {},
   "outputs": [],
   "source": [
    "t = np.arange(0, 2, 0.02);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "03f70d9e",
   "metadata": {},
   "outputs": [],
   "source": [
    "traj = mtraj(quintic, sol1.q, sol2.q, t);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "15b83e3a",
   "metadata": {},
   "outputs": [],
   "source": [
    "traj = mtraj(trapezoidal, sol1.q, sol2.q, t);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c83c52e7",
   "metadata": {},
   "outputs": [],
   "source": [
    "traj.q.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ff4ae368",
   "metadata": {},
   "outputs": [],
   "source": [
    "traj = jtraj(sol1.q, sol2.q, t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ff2b4fa0",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.plot(traj.q);  # this animation won't display in Jupyter"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a9ca35cf",
   "metadata": {},
   "outputs": [],
   "source": [
    "xplot(t, traj.q);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b2265f9b",
   "metadata": {},
   "outputs": [],
   "source": [
    "T = puma.fkine(traj.q);\n",
    "len(T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "be3cba5e",
   "metadata": {},
   "outputs": [],
   "source": [
    "p = T.t; \n",
    "p.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "16f8fa7d",
   "metadata": {},
   "outputs": [],
   "source": [
    "xplot(t, T.t, labels=\"x y z\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2601b340",
   "metadata": {},
   "outputs": [],
   "source": [
    "xplot(t, T.rpy(\"xyz\"), labels=\"roll pitch yaw\");"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b810878b",
   "metadata": {},
   "source": [
    "## 7.3.2 Cartesian Motion\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b0b98808",
   "metadata": {},
   "outputs": [],
   "source": [
    "Ts = ctraj(TE1, TE2, t);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a106e5df",
   "metadata": {},
   "outputs": [],
   "source": [
    "xplot(t, Ts.t, labels=\"x y z\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cb84821f",
   "metadata": {},
   "outputs": [],
   "source": [
    "xplot(t, Ts.rpy(\"xyz\"), labels=\"roll pitch yaw\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4bb951fe",
   "metadata": {},
   "outputs": [],
   "source": [
    "qc = puma.ikine_a(Ts);\n",
    "qc.q.shape"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9bde3ce3",
   "metadata": {},
   "source": [
    "## 7.3.3 Kinematics in a block diagram\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b41fce25",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "  %run -m jointspace -H -g  # no graphics\n",
    "else:\n",
    "  run_shell(\"jointspace\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f4e02126",
   "metadata": {},
   "source": [
    "## 7.3.4 Motion through a Singularity\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3027a20d",
   "metadata": {},
   "outputs": [],
   "source": [
    "TE1 = SE3.Trans(0.5, -0.3, 1.12) * SE3.OA((0, 1, 0), (1, 0, 0));\n",
    "TE2 = SE3.Trans(0.5, 0.3, 1.12) * SE3.OA((0, 1, 0), (1, 0, 0)); "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bcc9ef2e",
   "metadata": {},
   "outputs": [],
   "source": [
    "Ts = ctraj(TE1, TE2, t);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "10e6a4f3",
   "metadata": {},
   "outputs": [],
   "source": [
    "sol = puma.ikine_a(Ts, \"lu\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dde4c734",
   "metadata": {},
   "outputs": [],
   "source": [
    "xplot(t, sol.q, unwrap=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6cf91693",
   "metadata": {},
   "outputs": [],
   "source": [
    "m = puma.manipulability(sol.q);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b7bde5fd",
   "metadata": {},
   "source": [
    "## 7.3.5 Configuration Change\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e2ab8b73",
   "metadata": {},
   "outputs": [],
   "source": [
    "TE = SE3.Trans(0.4, 0.2, 0.6) * SE3.Rx(pi);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0ca16d3d",
   "metadata": {},
   "outputs": [],
   "source": [
    "sol_r = puma.ikine_a(TE, \"ru\");\n",
    "sol_l = puma.ikine_a(TE, \"lu\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f93cc861",
   "metadata": {},
   "outputs": [],
   "source": [
    "traj = jtraj(sol_r.q, sol_l.q, t);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4c62165b",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.plot(traj.q);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "19e22f49",
   "metadata": {},
   "source": [
    "# 7.4 Applications\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "45471d0d",
   "metadata": {},
   "source": [
    "## 7.4.1 Writing on a surface\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1c589266",
   "metadata": {},
   "outputs": [],
   "source": [
    "font = rtb_load_jsonfile(\"data/hershey.json\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b39bb6ab",
   "metadata": {},
   "outputs": [],
   "source": [
    "letter = font[\"B\"]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c48a98c8",
   "metadata": {},
   "outputs": [],
   "source": [
    "lift = 0.1; # height to raise the pen\n",
    "scale = 0.25;\n",
    "via = np.empty((0, 3));\n",
    "for stroke in letter[\"strokes\"]:\n",
    "  xyz = np.array(stroke) * scale # convert stroke to nx2 array\n",
    "  xyz = np.pad(xyz, ((0, 0), (0, 1))) # add third column, z=0\n",
    "  via = np.vstack((via, xyz))  # append rows to via points\n",
    "  via = np.vstack((via, np.hstack([xyz[-1,:2], lift]))) # lift pen"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4ceb9802",
   "metadata": {},
   "outputs": [],
   "source": [
    "xyz_traj = mstraj(via, qdmax=[0.5, 0.5, 0.5], q0=[0, 0, lift], \n",
    "                  dt=0.02, tacc=0.2).q;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ab33b5ec",
   "metadata": {},
   "outputs": [],
   "source": [
    "len(xyz_traj)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f1db9b26",
   "metadata": {},
   "outputs": [],
   "source": [
    "len(xyz_traj) * 0.02"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "35a4a56f",
   "metadata": {},
   "outputs": [],
   "source": [
    "fig = plt.figure(); ax = fig.add_subplot(111, projection=\"3d\");\n",
    "plt.plot(xyz_traj[:,0], xyz_traj[:,1], xyz_traj[:,2]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "aef54f1c",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_pen = SE3.Trans(0.6, 0, 0.7) * SE3.Trans(xyz_traj) * SE3.OA( [0, 1, 0], [0, 0, -1]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "78788557",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma = models.DH.Puma560();\n",
    "sol = puma.ikine_a(T_pen, \"lu\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a39943e9",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.plot(sol.q);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4c35cb55",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %run -m writing"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b4883f0e",
   "metadata": {},
   "source": [
    "## 7.4.2 A 4-Legged Walking robot\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a7fdc4f5",
   "metadata": {},
   "outputs": [],
   "source": [
    "mm = 0.001;  # millimeters\n",
    "L1 = 100 * mm;\n",
    "L2 = 100 * mm;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ef4f6cf4",
   "metadata": {},
   "outputs": [],
   "source": [
    "leg = ERobot(ET.Rz() * ET.Rx() * ET.ty(-L1) * ET.Rx() * ET.tz(-L2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4fc44b44",
   "metadata": {},
   "outputs": [],
   "source": [
    "leg.fkine([0,0,0]).t"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3b9cdb30",
   "metadata": {},
   "outputs": [],
   "source": [
    "leg.plot([0, 0, 0]);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6f3eb52a",
   "metadata": {},
   "source": [
    "### 7.4.2.1 Motion of One Leg\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "53248532",
   "metadata": {},
   "outputs": [],
   "source": [
    "xf = 50; xb = -xf;  y = -50; zu = -20; zd = -50;\n",
    "via = np.array([\n",
    "  [xf, y, zd],\n",
    "  [xb, y, zd],\n",
    "  [xb, y, zu],\n",
    "  [xf, y, zu],\n",
    "  [xf, y, zd]]) * mm;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "432d228f",
   "metadata": {},
   "outputs": [],
   "source": [
    "x = mstraj(via, tsegment=[3, 0.25, 0.5, 0.25], dt=0.01, tacc=0.1).q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dde9d2f2",
   "metadata": {},
   "outputs": [],
   "source": [
    "sol = leg.ikine_LM(SE3.Trans(x), mask=[1, 1, 1, 0, 0, 0]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "919a3de6",
   "metadata": {},
   "outputs": [],
   "source": [
    "leg.plot(sol.q);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "09f2c768",
   "metadata": {},
   "source": [
    "### 7.4.2.2 Motion of Four Legs\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1081144d",
   "metadata": {},
   "outputs": [],
   "source": [
    "W = 100 * mm; L = 200 * mm;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "de0ac592",
   "metadata": {},
   "outputs": [],
   "source": [
    "Tflip = SE3.Rz(pi);\n",
    "legs = [\n",
    "  ERobot(leg, name=\"leg0\", base=SE3.Trans( L/2,  W/2, 0)),\n",
    "  ERobot(leg, name=\"leg1\", base=SE3.Trans(-L/2,  W/2, 0)),\n",
    "  ERobot(leg, name=\"leg2\", base=SE3.Trans( L/2, -W/2, 0) * Tflip),\n",
    "  ERobot(leg, name=\"leg3\", base=SE3.Trans(-L/2, -W/2, 0) * Tflip)];"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b1ac1a7c",
   "metadata": {
    "lines_to_next_cell": 1
   },
   "outputs": [],
   "source": [
    "for i in range(4000):\n",
    "  legs[0].q = gait(qcycle, i, 0, False)\n",
    "  legs[1].q = gait(qcycle, i, 100, False)\n",
    "  legs[2].q = gait(qcycle, i, 200, True)\n",
    "  legs[3].q = gait(qcycle, i, 300, True)\n",
    "env.step(dt=0.02)  # render the graphics"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "944cbf27",
   "metadata": {
    "lines_to_next_cell": 1
   },
   "outputs": [],
   "source": [
    "def gait(cycle, k, phi, flip):\n",
    "  k = (k + phi) % cycle.shape[0]  # modulo addition\n",
    "  q = cycle[k, :]\n",
    "  if flip:\n",
    "    q[0] = -q[0]  # for right-side legs\n",
    "  return q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "16fb62b0",
   "metadata": {},
   "outputs": [],
   "source": [
    "%run -m walking"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cd698c8a",
   "metadata": {},
   "source": [
    "# 7.5 Advanced Topics\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7ab671d6",
   "metadata": {},
   "source": [
    "## 7.5.2 Creating the Kinematic Model for a Robot\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ea42256b",
   "metadata": {},
   "outputs": [],
   "source": [
    "L1 = 0.672; L2 = -0.2337; L3 = 0.4318;\n",
    "L4 = 0.0203; L5 = 0.0837; L6 = 0.4318;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e938cfed",
   "metadata": {},
   "outputs": [],
   "source": [
    "e = ET.tz(L1) * ET.Rz() * ET.ty(L2) * ET.Ry() \\\n",
    "   * ET.tz(L3) * ET.tx(L4) * ET.ty(L5) * ET.Ry() \\\n",
    "   * ET.tz(L6) * ET.Rz() * ET.Ry() * ET.Rz();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7c82b8d6",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = ERobot(e)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5c7ba783",
   "metadata": {},
   "source": [
    "## 7.5.3 Modified Denavit-Hartenberg Parameters\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "912c00b0",
   "metadata": {},
   "outputs": [],
   "source": [
    "L1 = RevoluteMDH(d=1)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "462c99d5",
   "metadata": {},
   "source": [
    "## 7.5.4 Products of exponentials\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "67077079",
   "metadata": {},
   "outputs": [],
   "source": [
    "a1 = 1; a2 = 1;\n",
    "TE0 = SE2(a1 + a2, 0, 0);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "55fb33f2",
   "metadata": {},
   "outputs": [],
   "source": [
    "S0 = Twist2.UnitRevolute([0, 0]);\n",
    "S1 = Twist2.UnitRevolute([a1, 0]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1bf5370d",
   "metadata": {},
   "outputs": [],
   "source": [
    "TE = S0.exp(np.deg2rad(30)) * S1.exp(np.deg2rad(40)) * TE0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "acc40bed",
   "metadata": {},
   "outputs": [],
   "source": [
    "irb140 = models.DH.IRB140();\n",
    "S, TE0 = irb140.twists()\n",
    "S"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5456e9a1",
   "metadata": {},
   "outputs": [],
   "source": [
    "T = S.exp(irb140.qr).prod() * TE0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b7236820",
   "metadata": {},
   "outputs": [],
   "source": [
    "irb140.plot(irb140.qz);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "193e1d10",
   "metadata": {},
   "outputs": [],
   "source": [
    "lines = S.line()\n",
    "lines.plot(\"k:\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "01cdc766",
   "metadata": {},
   "outputs": [],
   "source": [
    "link1 = PoERevolute([0, 0, 1], [0, 0, 0]);  # rotate about z-axis, through (0,0,0)\n",
    "link2 = PoERevolute([0, 0, 1], [1, 0, 0]);  # rotate about z-axis, through (1,0,0)\n",
    "TE0 = SE3.Tx(2);  # end-effector pose when q=[0,0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9c9e647b",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = PoERobot([link1, link2], TE0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3fce4dbd",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.fkine([0, 0]).printline()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ad570f11",
   "metadata": {},
   "source": [
    "## 7.5.5 Collision detection\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f834c8bb",
   "metadata": {},
   "outputs": [],
   "source": [
    "panda = models.URDF.Panda();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ee87845f",
   "metadata": {},
   "outputs": [],
   "source": [
    "from spatialgeometry import Cuboid\n",
    "box = Cuboid([1, 1, 1], pose=SE3.Tx(1.1));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "62ef2561",
   "metadata": {},
   "outputs": [],
   "source": [
    "panda.iscollided(panda.qr, box)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "58304e2b",
   "metadata": {},
   "outputs": [],
   "source": [
    "box.T = SE3.Tx(0.2)\n",
    "panda.iscollided(panda.qr, box)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "85130c71",
   "metadata": {},
   "outputs": [],
   "source": [
    "# plot robot and get reference to graphics environment\n",
    "env = panda.plot(panda.qr, backend=\"swift\");  \n",
    "env.add(box);  # add box to graphics\n",
    "env.step()     # update the graphics"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "eec3368d",
   "metadata": {},
   "outputs": [],
   "source": [
    "env.step()"
   ]
  }
 ],
 "metadata": {
  "jupytext": {
   "cell_metadata_filter": "-all",
   "main_language": "python",
   "notebook_metadata_filter": "-all"
  },
  "kernelspec": {
   "display_name": "dev",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.9"
  },
  "vscode": {
   "interpreter": {
    "hash": "b7d6b0d76025b9176285a6442c3dd6dd39bcfe7241029b7898b7106bd5e9b472"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
